/*! \file HXMatrix.h
    \brief Matrices, vectors, and other mathematical constructs.

	Head X | Flinders University | Martin Luerssen | 2009+
*/

#pragma once

template<class T, const size_t size> class HXVector;
typedef HXVector<float, 2> HXVector2F;
typedef HXVector<float, 3> HXVector3F;
typedef HXVector<unsigned int, 3> HXVectorTriangle;
typedef HXVector<unsigned int, 4> HXVectorQuad;
template<class T, const size_t rows, const size_t columns> class HXMatrix;
typedef HXMatrix<float, 3, 3> HXMatrix3F;
typedef HXMatrix<float, 4, 4> HXMatrix4F;

#include <vector>
#include <cmath>
#include <iostream>
using namespace std;

//-----------------------------------------------------------------------------

//! Vector base template.
template<class T, const size_t size> 
class HXVector
{
public:
	HXVector() { Set(static_cast<T>(0)); }
	HXVector(const T& x0, const T& x1) { Set(static_cast<T>(0)); x[0] = x0; x[1] = x1; }
	HXVector(const T& x0, const T& x1, const T& x2) { Set(static_cast<T>(0)); x[0] = x0; x[1] = x1; x[2] = x2; }
	HXVector(const T& x0, const T& x1, const T& x2, const T& x3) { Set(static_cast<T>(0)); x[0] = x0; x[1] = x1; x[2] = x2; x[3] = x3; }
	HXVector(const HXVector<T, size>& other) { for (size_t i = 0; i < size; i++) x[i] = other.x[i]; }
	T&			operator[](size_t i) { return x[i]; }
	const T&	operator[](size_t i) const { return x[i]; }
	T*			GetData() { return x; }
	T			Length() const;
	size_t		Size() const { return size; }
	void		Set(T defVal) { for (size_t i = 0; i < size; i++) x[i] = defVal; }

	HXVector<T, size>&	operator=(const HXVector<T, size>& other) { for (size_t i = 0; i < size; i++) x[i] = other.x[i]; return *this; }
	HXVector<T, size>&	operator+=(const HXVector<T, size>& other);
	HXVector<T, size>&	operator-=(const HXVector<T, size>& other);
	HXVector<T, size>&	operator*=(const HXVector<T, size>& other);
	HXVector<T, size>&	operator/=(const HXVector<T, size>& other);
	HXVector<T, size>&	operator+=(const T& factor);
	HXVector<T, size>&	operator-=(const T& factor);
	HXVector<T, size>&	operator*=(const T& factor);
	HXVector<T, size>&	operator/=(const T& factor);
	bool		operator==(const HXVector<T, size>& other) const;
	T			Dot(const HXVector<T, size>& other) const;

	void		FromBinaryStream(istream& in)
	{
		for (size_t i = 0; i < size; i++)
			in.read(reinterpret_cast<char *>(&x[i]), sizeof(T));
	}
	void		ToBinaryStream(ostream& out)
	{
		for (size_t i = 0; i < size; i++)
			out.write(reinterpret_cast<char *>(&x[i]), sizeof(T));
	}

protected:
    T x[size];
};

//-----------------------------------------------------------------------------

//! Matrix base template.
template<class T, const size_t rows, const size_t columns> 
class HXMatrix
{
public:
	HXMatrix() { Set(static_cast<T>(0)); }
	HXMatrix(const HXMatrix<T, rows, columns>& other) { for (size_t i = 0; i < rows; i++) m[i] = other.m[i]; }
	HXVector<T, columns>& operator[](size_t i) { return m[i]; }
	const HXVector<T, columns>& operator[](size_t i) const { return m[i]; }
	HXVector<T, columns>* GetData() { return m; }
	size_t SizeRows() const { return rows; }
	size_t SizeColumns() const { return columns; }
    void Set(T defVal) { for (size_t i = 0; i < rows; i++) m[i].Set(defVal); }
    void SetIdentity();

	HXMatrix<T, rows, columns>& operator=(const HXMatrix<T, rows, columns>& other) { for (size_t i = 0; i < rows; i++) m[i] = other.m[i]; return *this; }
	HXMatrix<T, rows, columns>& operator+=(const HXMatrix<T, rows, columns>& other);
    HXMatrix<T, rows, columns>& operator-=(const HXMatrix<T, rows, columns>& other);
	HXMatrix<T, rows, columns>& operator*=(const T& factor);
	bool operator==(const HXMatrix<T, rows, columns>& other) const;

protected:
	HXVector<T, columns> m[rows];
};

HXMatrix3F RotMatrixX(float theta);
HXMatrix3F RotMatrixY(float theta);
HXMatrix3F RotMatrixZ(float theta);
HXMatrix3F RotMatrixAxis(const HXVector3F& axis, float theta);

//-----------------------------------------------------------------------------

// free-standing operators:

template<class T, const size_t size>
inline
HXVector<T, size> operator+(const HXVector<T, size>& lhs, const HXVector<T, size>& rhs)
{
	HXVector<T,size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] + rhs[i];
	}
	return result;
}

template<class T, const size_t size>
inline
HXVector<T, size> operator-(const HXVector<T, size>& lhs, const HXVector<T, size>& rhs)
{
	HXVector<T,size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] - rhs[i];
	}
	return result;
}

template<class T, const size_t size>
inline
HXVector<T, size> operator*(const HXVector<T, size>& lhs, const HXVector<T, size>& rhs)
{
	HXVector<T,size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] * rhs[i];
	}
	return result;
}

template<class T, const size_t size>
inline
HXVector<T, size> operator/(const HXVector<T, size>& lhs, const HXVector<T, size>& rhs)
{
	HXVector<T,size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] / rhs[i];
	}
	return result;
}

template<class T, const size_t size>
inline
HXVector<T, size> operator+(const HXVector<T, size>& lhs, const T& rhs)
{
	HXVector<T,size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] + rhs;
	}
	return result;
}

template<class T, const size_t size>
inline
HXVector<T, size> operator-(const HXVector<T, size>& lhs, const T& rhs)
{
	HXVector<T,size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] - rhs;
	}
	return result;
}

template<class T, const size_t size>
inline
HXVector<T, size> operator*(const HXVector<T, size>& lhs, const T& rhs)
{
	HXVector<T,size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] * rhs;
	}
	return result;
}

template<class T, const size_t size>
inline
HXVector<T, size> operator/(const HXVector<T, size>& lhs, const T& rhs)
{
	HXVector<T, size> result;
	for (size_t i = 0; i < size; i++)
	{
		result[i] = lhs[i] / rhs;
	}
	return result;
}

//------------
	
template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns> operator+(const HXMatrix<T, rows, columns>& lhs, const HXMatrix<T, rows, columns>& rhs)
{
    HXMatrix<T, rows, columns> new_m;
    for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj = 0; jj < columns; jj++)
		{
            new_m[ii][jj] = lhs[ii][jj] + rhs[ii][jj];
		}
	}
    return new_m;
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns> operator-(const HXMatrix<T, rows, columns>& lhs, const HXMatrix<T, rows, columns>& rhs)
{
    HXMatrix<T, rows, columns> new_m;
    for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj = 0; jj < columns; jj++)
		{
            new_m[ii][jj] = lhs[ii][jj] - rhs[ii][jj];
		}
	}
    return new_m;
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns> operator-(const HXMatrix<T, rows, columns>& lhs)
{
	HXMatrix<T, rows, columns> new_m;
	for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj = 0; jj < columns; jj++)
		{
            new_m[ii][jj] = -(lhs[ii][jj]);
		}
	}
    return new_m;
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, rows> operator*(const HXMatrix<T, rows, columns>& lhs, const HXMatrix<T, columns, rows>& rhs)
{
	HXMatrix<T, rows, rows> new_m;
    for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj=0; jj < rows; jj++)
        {
            new_m[ii][jj] = static_cast<T>(0);
            for (size_t kk = 0; kk < columns; kk++)
			{
                new_m[ii][jj] += lhs[ii][kk] * rhs[kk][jj];
			}
        }
	}
    return new_m;
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns> operator*(const HXMatrix<T, rows, columns>& lhs, const T& rhs)
{
	HXMatrix<T, rows, columns> new_m;
	for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj = 0; jj < columns; jj++)
		{
            new_m[ii][jj] = rhs * m[ii][jj];
		}
	}
    return new_m;
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns> Transpose(const HXMatrix<T, rows, columns>& lhs)
{
    HXMatrix<T, rows, columns> trans_m;
    for (size_t ii = 0; ii < rows; ii++)
	{
		for (size_t jj = 0; jj < columns; jj++)
		{
            trans_m[jj][ii] = lhs[ii][jj];
		}
	}
    return trans_m;
}

//-----------------------------------------------------------------------------

template<class T, const size_t size> 
inline 
T HXVector<T, size>::Length() const 
{
	T sum = static_cast<T>(0);
	for (size_t i = 0; i < size; i++)
	{
		sum += x[i] * x[i];
	}
	return static_cast<T>(sqrt(sum));
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator+=(const HXVector<T, size> &other)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] += other[i];
	}
	return *this;
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator-=(const HXVector<T, size> &other)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] -= other[i];
	}
	return *this;
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator*=(const HXVector<T, size> &other)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] *= other[i];
	}
	return *this;
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator/=(const HXVector<T, size> &other)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] /= other[i];
	}
	return *this;
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator+=(const T &factor)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] += factor;
	}
	return *this;
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator-=(const T &factor)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] -= factor;
	}
	return *this;
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator*=(const T &factor)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] *= factor;
	}
	return *this;
}

template<class T, const size_t size> 
inline 
HXVector<T, size>& HXVector<T, size>::operator/=(const T &factor)
{
	for (size_t i = 0; i < size; i++)
	{
		x[i] /= factor;
	}
	return *this;
}

template<class T, const size_t size> 
inline 
bool HXVector<T, size>::operator==(const HXVector<T, size> &other) const
{
	for (size_t i = 0; i < size; i++)
	{
		if (x[i] != other[i])
		{
			return false;
		}
	}
	return true;
}

template<class T, const size_t size> 
inline 
T HXVector<T, size>::Dot(const HXVector<T, size> &other) const
{
	T result = static_cast<T>(0);
	for (size_t i = 0; i < size; i++)
	{
		result += x[i] * other[i];
	}
	return result;
}

//-----------------------------------------------------------------------------

// free-standing specialized functions:

inline 
float Cross(const HXVector2F& x, const HXVector2F& other)
{
    return x[0] * other[1] - x[1] * other[0];
}

inline
HXVector2F Rotate(const HXVector2F& x, float theta)
{
    float cosine = cos(theta);
    float sine = sin(theta);
    
	HXVector2F result;
    result[0] = x[0] * cosine - x[1] * sine;
    result[1] = x[1] * cosine + x[0] * sine;

    return result;
}

//------------

inline 
HXVector3F Cross(const HXVector3F& x, const HXVector3F& other)
{
    return HXVector3F(x[1] * other[2] - x[2] * other[1], x[2] * other[0] - x[0] * other[2], x[0] * other[1] - x[1] * other[0]);
}

inline 
HXVector3F RotateX(const HXVector3F& x, float theta)
{
    float cosine = cos(theta);
	float sine	= sin(theta);   
    return HXVector3F(x[0], x[1] * cosine - x[2] * sine, x[2] * cosine + x[1] * sine);
}

inline 
HXVector3F RotateY(const HXVector3F& x, float theta)
{
    float cosine = cos(theta);
	float sine	= sin(theta);   
    return HXVector3F(x[0] * cosine + x[2] * sine, x[1], x[2] * cosine - x[0] * sine);
}

inline 
HXVector3F RotateZ(const HXVector3F& x, float theta)
{
    float cosine = cos(theta);
	float sine	= sin(theta);   
    return HXVector3F(x[0] * cosine - x[1] * sine, x[1] * cosine + x[0] * sine, x[2]);
}

inline 
HXVector3F RotateAxis(const HXVector3F& x, const HXVector3F& axis, float theta)
{
	float cosine = cos(theta);
    float sine = sin(theta);
	float vtheta = 1.0f - cosine;
   
	float length = axis.Length();
    HXVector3F k = axis / length;

	HXVector3F result;
    result[0] =
        (k[0] * k[0] * vtheta + cosine) * x[0] +
        (k[0] * k[1] * vtheta - k[2] * sine) * x[1] +
        (k[0] * k[2] * vtheta + k[1] * sine) * x[2];
    result[1] =
        (k[0] * k[1] * vtheta + k[2] * sine) * x[0] +
        (k[1] * k[1] * vtheta + cosine) * x[1] +
        (k[1] * k[2] * vtheta - k[0] * sine) * x[2];
    result[2] =
        (k[0] * k[2] * vtheta - k[1] * sine) * x[0] +
        (k[1] * k[2] * vtheta + k[0] * sine) * x[1] +
        (k[2] * k[2] * vtheta + cosine) * x[2];

    return result;
}

//-----------------------------------------------------------------------------

template<class T, const size_t rows, const size_t columns> 
inline
void HXMatrix<T, rows, columns>::SetIdentity()
{
    size_t nn = (rows < columns) ? rows : columns;
	Set(static_cast<T>(0));
    for (size_t ii = 0; ii < nn; ii++)
	{
        m[ii][ii] = static_cast<T>(1);
	}
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns>& HXMatrix<T, rows, columns>::operator+=(const HXMatrix<T, rows, columns>& other)
{
    for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj = 0; jj < columns; jj++)
		{
            m[ii][jj] += other[ii][jj];
		}
	}
    return *this;
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns>& HXMatrix<T, rows, columns>::operator-=(const HXMatrix<T, rows, columns>& other)
{
    for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj = 0; jj < columns; jj++)
		{
            m[ii][jj] -= other[ii][jj];
		}
	}
    return *this;
}

template<class T, const size_t rows, const size_t columns> 
inline
HXMatrix<T, rows, columns>& HXMatrix<T, rows, columns>::operator*=(const T& factor)
{
	for (size_t ii = 0; ii < rows; ii++)
	{
        for (size_t jj = 0; jj < columns; jj++)
		{
            m[ii][jj] *= factor;
		}
	}
    return *this;
}

template<class T, const size_t rows, const size_t columns> 
inline
bool HXMatrix<T, rows, columns>::operator==(const HXMatrix<T, rows, columns>& other) const
{
	bool result = true;
	for (size_t ii = 0; ii < rows; ii++)
	{
		for (size_t jj = 0; jj < columns; jj++)
		{
			result = result && (m[ii][jj] == other[ii][jj]);
		}
	}
	return result;
}

//-----------------------------------------------------------------------------

inline
HXMatrix3F RotMatrixX(float theta)
{
	float cosine = cos(theta);
    float sine = sin(theta);
    HXMatrix3F mat;
    mat[0][0] = 1.0;   mat[0][1] = 0.0;     mat[0][2] = 0.0;
    mat[1][0] = 0.0;   mat[1][1] = cosine;  mat[1][2] = -sine;
    mat[2][0] = 0.0;   mat[2][1] = sine;    mat[2][2] = cosine;
    return mat;
}

inline
HXMatrix3F RotMatrixY(float theta)
{
	float cosine = cos(theta);
    float sine = sin(theta);
    HXMatrix3F mat;
    mat[0][0]=cosine;  mat[0][1]=0.0;   mat[0][2]=sine;
    mat[1][0]=0.0;     mat[1][1]=1.0;   mat[1][2]=0.0;
    mat[2][0]=-sine;   mat[2][1]=0.0;   mat[2][2]=cosine;
    return mat;
}

inline
HXMatrix3F RotMatrixZ(float theta)
{
	float cosine = cos(theta);
    float sine = sin(theta);
    HXMatrix3F mat;
    mat[0][0]=cosine;  mat[0][1]=-sine;   mat[0][2]=0.0;
    mat[1][0]=sine;    mat[1][1]=cosine;  mat[1][2]=0.0;
    mat[2][0]=0.0;     mat[2][1]=0.0;     mat[2][2]=1.0;
    return mat;
}

inline
HXMatrix3F RotMatrixAxis(const HXVector3F& axis, float theta)
{
	float cosine = cos(theta);
    float sine = sin(theta);
    float vtheta = 1.0f - cosine;
    HXMatrix3F mat;
    mat[0][0] = axis[0] * axis[0] * vtheta + cosine;
    mat[0][1] = axis[0] * axis[1] * vtheta - axis[2] * sine;
    mat[0][2] = axis[0] * axis[2] * vtheta + axis[1] * sine;
    mat[1][0] = axis[0] * axis[1] * vtheta + axis[2] * sine;
    mat[1][1] = axis[1] * axis[1] * vtheta + cosine;
    mat[1][2] = axis[1] * axis[2] * vtheta - axis[0] * sine;
    mat[2][0] = axis[0] * axis[2] * vtheta - axis[1] * sine;
    mat[2][1] = axis[1] * axis[2] * vtheta + axis[0] * sine;
    mat[2][2] = axis[2] * axis[2] * vtheta + cosine;
    return mat;
}

//-----------------------------------------------------------------------------
